// Implementação do Kalman Filter

// Variaveis de Entrada
// Média do Estado Anterior -> Inicializa com a posição inicial do Robo (X_robo,Y_robo,Theta_robo)
// Covariancia do Estado Anterior (3x3) -> Inicializada como Matriz Nula
// Controle no instante atual -> (Velocidade Transversal e Velocidade Angular)
// Leituras dos Sensores -> Conjunto de vetores (dist,theta,signature) para cada Landmark encontrado
// Mapa -> Lista Encadeada Simples que contém o vetor (mx,my,signature) para cada Landmark do mapa

#include "kalman.h" 

// Algoritmo baseado na Pag. 204 - Probabilistic Robots ( Sebastian Thrun )
void Kalman_Filter(PGMATRIX Matriz_Mediat, PGMATRIX Matriz_Covt, double controle[], int signature[], list<lista_mapa> *Lista_M, list<lista_sensor> *Lista_S, double d_t)
{
  
  /*
  double random = (0.001*rand()/RAND_MAX-0.0005);
  //std::cout << "RANDOM = " << random << std::endl;
  controle[1] = (controle[1] == 0) ? random : controle[1];   */
  
  // Variaveis
  
  double theta_robo;
  double pos_landmark[2];
  double z_landmark[3];
  double media_t[3];
  double raiz_q;
  int cont;

  z_landmark[2] = 0.0;
  
  PGMATRIX Matriz_Gt;				
  PGMATRIX Matriz_Vt;				
  PGMATRIX Matriz_Mt;
  PGMATRIX Matriz_Qt;
  PGMATRIX Matriz_Kt;
  PGMATRIX Matriz_Ht;
  PGMATRIX Matriz_I;
  

  // Alocando Memória para as Matrizes

  Matriz_Gt = PGMATRIX_ALLOC(3,3);
  Matriz_Vt = PGMATRIX_ALLOC(3,2);
  Matriz_Mt = PGMATRIX_ALLOC(2,2);
  Matriz_Qt = PGMATRIX_ALLOC(3,3);
  Matriz_Kt = PGMATRIX_ALLOC(3,3);
  Matriz_Ht = PGMATRIX_ALLOC(3,3);
  Matriz_I = PGMATRIX_ALLOC(3,3);
  
  PGMATRIX_IDENTITY(Matriz_I); 

  // Acochambrando para a Media
  
  media_t[0] = PGMATRIX_DATA(Matriz_Mediat, 1, 1);
  media_t[1] = PGMATRIX_DATA(Matriz_Mediat, 2, 1);
  media_t[2] = PGMATRIX_DATA(Matriz_Mediat, 3, 1);

  // Linha 2  
  theta_robo = media_t[2]; 

  // Prediction Step (Linhas 3-7)

  // Linha 3
  Gera_Matriz_Gt(Matriz_Gt,controle,theta_robo,d_t);

  // Linha 4
  Gera_Matriz_Vt(Matriz_Vt,controle,theta_robo,d_t);

  // Linha 5
  Gera_Matriz_Mt(Matriz_Mt,controle);

  // Linha 6
  Atualiza_Media_Controle(media_t,controle,theta_robo,d_t);

  // Linha 7
  Atualiza_Covariancia_Controle(Matriz_Gt, Matriz_Vt, Matriz_Mt, Matriz_Covt);
  
  // Linha 8
  Gera_Matriz_Qt(Matriz_Qt);
  
  // Linha 9 - 17
       
  for(cont = 0; cont<6; cont++)	
  {
    // For all observed features - Linha 10
    if (signature[cont] == 1)
    {
      Ler_Lista_Mapa(Lista_M,cont,pos_landmark);
      Ler_Lista_Sensor(Lista_S,cont,z_landmark);
     
      // Linha 11 e Linha 13
      raiz_q = Gera_Matriz_Ht(Matriz_Ht,pos_landmark,media_t);

      // Linha 14 - 15
      Gera_Matriz_Kt(Matriz_Kt,Matriz_Ht,Matriz_Qt,Matriz_Covt);
      
      //Linha 12 e Linha 16 - 17
      Atualiza_Media_Covariancia_Sensores(media_t, pos_landmark, z_landmark, Matriz_Kt, Matriz_Covt,Matriz_Ht,Matriz_I,raiz_q);
    }
  }    
  
  PGMATRIX_DATA(Matriz_Mediat, 1, 1) = media_t[0];
  PGMATRIX_DATA(Matriz_Mediat, 2, 1) = media_t[1];
  PGMATRIX_DATA(Matriz_Mediat, 3, 1) = media_t[2];

  // Liberando Memoria
  
  PGMATRIX_FREE(Matriz_Gt);
  PGMATRIX_FREE(Matriz_Vt);
  PGMATRIX_FREE(Matriz_Mt);
  PGMATRIX_FREE(Matriz_Qt);
  PGMATRIX_FREE(Matriz_Kt);
  PGMATRIX_FREE(Matriz_Ht);
  PGMATRIX_FREE(Matriz_I);
  
}

void Gera_Matriz_Gt(PGMATRIX Matriz_Gt, double controle[], double theta_robo, double d_t)	
{
  // Variaveis
  
  double aux1;

  aux1 = (controle[0])/(controle[1]);	// aux1 = (velocidade transversal)/(velocidade angular)

  // Preenchendo a Matriz Gt

  PGMATRIX_DATA(Matriz_Gt, 1, 1) = PGMATRIX_DATA(Matriz_Gt, 2, 2) =   PGMATRIX_DATA(Matriz_Gt, 3, 3) = 1.0;
  PGMATRIX_DATA(Matriz_Gt, 1, 2) = PGMATRIX_DATA(Matriz_Gt, 2, 1) =   PGMATRIX_DATA(Matriz_Gt, 3, 1) = PGMATRIX_DATA(Matriz_Gt, 3, 2) = 0.0;
 
  if(controle[1] != 0 )
  { 
    PGMATRIX_DATA(Matriz_Gt, 1, 3) = (-1)*(aux1)*(cos(theta_robo)) + aux1*(cos(theta_robo + controle[1]*d_t));
    PGMATRIX_DATA(Matriz_Gt, 2, 3) = (-1)*(aux1)*(sin(theta_robo)) + aux1*(sin(theta_robo + controle[1]*d_t));
  }
  
  // Tratando o caso em que Velocidade Angular seja igual a zero
  else
  {
    PGMATRIX_DATA(Matriz_Gt, 1, 3) = (-1)*(controle[0])*(sin(theta_robo))*d_t;
    PGMATRIX_DATA(Matriz_Gt, 2, 3) = (controle[0])*(cos(theta_robo))*d_t;
  }

  /*
  std::cout << "Matriz Gt\n";
  std:: cout << PGMATRIX_DATA(Matriz_Gt,1,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Gt,1,2) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Gt,1,3) << " \n";
  std:: cout << PGMATRIX_DATA(Matriz_Gt,2,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Gt,2,2) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Gt,2,3) << " \n";
  std:: cout << PGMATRIX_DATA(Matriz_Gt,3,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Gt,3,2) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Gt,3,3) << " \n"; */
  
}

void Gera_Matriz_Vt(PGMATRIX Matriz_Vt, double controle[], double theta_robo, double d_t)	
{
  // Variaveis
  
  double aux1,aux2,aux3,aux4;

  aux1 = (controle[1]*d_t);		
  aux4 = (controle[0])/(controle[1]);

  // Preenchendo a Matriz Mt
  
  PGMATRIX_DATA(Matriz_Vt, 3, 1) = 0.0;
  PGMATRIX_DATA(Matriz_Vt, 3, 2) = d_t;

  if(controle[1] != 0 )
  { 
    aux2 = (-1)*sin(theta_robo) + sin(theta_robo + aux1);
    PGMATRIX_DATA(Matriz_Vt, 1, 1) = aux2/(controle[1]);

    aux3 = cos(theta_robo) - cos(theta_robo + aux1);
    PGMATRIX_DATA(Matriz_Vt, 2, 1) = aux3/(controle[1]);

    PGMATRIX_DATA(Matriz_Vt, 1, 2) = (controle[0]*aux2*(-1))/(controle[1]*controle[1]);
    PGMATRIX_DATA(Matriz_Vt, 1, 2) = PGMATRIX_DATA(Matriz_Vt, 1, 2) + aux4*d_t*cos(theta_robo + aux1);

    PGMATRIX_DATA(Matriz_Vt, 2, 2) = (controle[0]*aux3*(-1))/(controle[1]*controle[1]);
    PGMATRIX_DATA(Matriz_Vt, 2, 2) = PGMATRIX_DATA(Matriz_Vt, 2, 2) + aux4*d_t*sin(theta_robo + aux1);
  }
  
  // Tratando o caso em que Velocidade Angular seja igual a zero
  else
  {
    PGMATRIX_DATA(Matriz_Vt, 1, 1) = (d_t)*cos(theta_robo);
    PGMATRIX_DATA(Matriz_Vt, 2, 1) = (-0.5)*(d_t)*(d_t)*sin(theta_robo);
    PGMATRIX_DATA(Matriz_Vt, 1, 2) = (d_t)*sin(theta_robo);
    PGMATRIX_DATA(Matriz_Vt, 2, 2) = (-0.5)*(d_t)*(d_t)*cos(theta_robo);
  }
  

  /*
  std::cout << "Matriz Vt\n";
  std:: cout << PGMATRIX_DATA(Matriz_Vt,1,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Vt,1,2) << " \n";
  std:: cout << PGMATRIX_DATA(Matriz_Vt,2,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Vt,2,2) << " \n";
  std:: cout << PGMATRIX_DATA(Matriz_Vt,3,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Vt,3,2) << " \n"; */

}

void Gera_Matriz_Mt(PGMATRIX Matriz_Mt, double controle[])	
{
  // Variaveis

  PGMATRIX_DATA(Matriz_Mt, 1, 1) = alpha1*(controle[0]*controle[0]) + alpha2*(controle[1]*controle[1]); 
  PGMATRIX_DATA(Matriz_Mt, 1, 2) = 0.0;
  PGMATRIX_DATA(Matriz_Mt, 2, 1) = 0.0;
  PGMATRIX_DATA(Matriz_Mt, 2, 2) = alpha3*(controle[0]*controle[0]) + alpha4*(controle[1]*controle[1]);

  /*  
  std::cout << "Matriz Mt\n";
  std:: cout << PGMATRIX_DATA(Matriz_Mt,1,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Mt,1,2) << " \n";
  std:: cout << PGMATRIX_DATA(Matriz_Mt,2,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Mt,2,2) << " \n"; */
}

void Atualiza_Media_Controle(double media_t[], double controle[], double theta_robo, double d_t)	
{
  // Variaveis
 
  double aux1,aux2;

  aux1 = (controle[0]/controle[1]);
  aux2 = (controle[1]*d_t);

  // Atualizando a Media_t a partir do Controle fornecido (Control Update - Parte 1)
  if(controle[1] != 0 )
  {
    media_t[0] = media_t[0] + ( ((-1)*aux1*sin(theta_robo)) + (aux1*sin(theta_robo + aux2)) );
    media_t[1] = media_t[1] + ( (aux1*cos(theta_robo)) - (aux1*cos(theta_robo + aux2)) );
  }
  
  // Tratando o caso em que Velocidade Angular seja igual a zero
  else
  {
    media_t[0] = media_t[0] + controle[0]*cos(theta_robo)*(d_t);
    media_t[1] = media_t[1] + controle[0]*sin(theta_robo)*(d_t);
  
  }
  media_t[2] = media_t[2] + aux2;
}

void Atualiza_Covariancia_Controle(PGMATRIX Matriz_Gt, PGMATRIX Matriz_Vt, PGMATRIX Matriz_Mt, PGMATRIX Matriz_Covt) 
{
  PGMATRIX Matriz_Gt_Trans;
  PGMATRIX Matriz_Vt_Trans;
  PGMATRIX Matriz_Aux;
  PGMATRIX Matriz_Aux1;
  PGMATRIX Matriz_Aux2;
  PGMATRIX Matriz_Aux3;
  
  // Alocando Memoria para as Matrizes Transpostas e para as Auxiliares
  
  Matriz_Gt_Trans = PGMATRIX_ALLOC(3,3);
  Matriz_Vt_Trans = PGMATRIX_ALLOC(2,3);
  Matriz_Aux = PGMATRIX_ALLOC(3,3);
  Matriz_Aux1 = PGMATRIX_ALLOC(3,3);
  Matriz_Aux2 = PGMATRIX_ALLOC(3,3);  
  Matriz_Aux3 = PGMATRIX_ALLOC(2,3); 
    
  // Calculando as Matriz Transpostas de Gt e de Vt
  
  PGMATRIX_TRANSPOSE_COPY(Matriz_Gt_Trans, Matriz_Gt);
  PGMATRIX_TRANSPOSE_COPY(Matriz_Vt_Trans, Matriz_Vt);  
  
  // Atualizando a Cov_t a partir do Controle fornecido (Control Update - Parte 2)

  PGMATRIX_TRIPLEMULTIPLY_COPY(Matriz_Aux, Matriz_Gt, Matriz_Covt, Matriz_Gt_Trans, Matriz_Aux1); 	// Aux = Gt*Cov_(t-1)*Gt_Trans
  PGMATRIX_TRIPLEMULTIPLY_COPY(Matriz_Aux2, Matriz_Vt, Matriz_Mt, Matriz_Vt_Trans, Matriz_Aux3);	// Aux2 = Vt*Mt*Vt_Trans
  PGMATRIX_ADD_COPY(Matriz_Covt, Matriz_Aux, Matriz_Aux2);						// Cov_t = Aux + Aux2
  
  // Liberando a Memória
  
  PGMATRIX_FREE(Matriz_Gt_Trans);
  PGMATRIX_FREE(Matriz_Vt_Trans);
  PGMATRIX_FREE(Matriz_Aux);
  PGMATRIX_FREE(Matriz_Aux1);
  PGMATRIX_FREE(Matriz_Aux2);
  PGMATRIX_FREE(Matriz_Aux3);
  
}

void Gera_Matriz_Qt(PGMATRIX Matriz_Qt) 
{

  // Preenchendo a Matriz Qt

  PGMATRIX_DATA(Matriz_Qt, 1, 1) = pow(sigma_r,2);
  PGMATRIX_DATA(Matriz_Qt, 2, 2) = pow(sigma_phi,2);
  PGMATRIX_DATA(Matriz_Qt, 3, 3) = pow(sigma_s,2);
  PGMATRIX_DATA(Matriz_Qt, 1, 2) = PGMATRIX_DATA(Matriz_Qt, 2, 1) =  PGMATRIX_DATA(Matriz_Qt, 3, 1) = 0.0;
  PGMATRIX_DATA(Matriz_Qt, 1, 3) = PGMATRIX_DATA(Matriz_Qt, 2, 3) =  PGMATRIX_DATA(Matriz_Qt, 3, 2) = 0.0;
  
}

double Gera_Matriz_Ht(PGMATRIX Matriz_Ht, double pos_landmark[], double media_t[])
{
  // Variaveis

  double q;
  double raiz_q;

  // Linha 11
  
  q = pow((pos_landmark[0] - media_t[0]),2) + pow((pos_landmark[1] - media_t[1]),2);
  raiz_q = sqrt(q);

  // Linha 13
  
  PGMATRIX_DATA(Matriz_Ht, 1, 3) = PGMATRIX_DATA(Matriz_Ht, 3, 1) = PGMATRIX_DATA(Matriz_Ht, 3, 2) =  PGMATRIX_DATA(Matriz_Ht, 3, 3) = 0.0;
  PGMATRIX_DATA(Matriz_Ht, 2, 3) = -1.0;

  PGMATRIX_DATA(Matriz_Ht, 1, 1) =  ((-1)*(pos_landmark[0] - media_t[0])/raiz_q);
  PGMATRIX_DATA(Matriz_Ht, 1, 2) =  ((-1)*(pos_landmark[1] - media_t[1])/raiz_q);
  
  PGMATRIX_DATA(Matriz_Ht, 2, 1) =  ((pos_landmark[1] - media_t[1])/q);
  PGMATRIX_DATA(Matriz_Ht, 2, 2) =  ((-1)*(pos_landmark[0] - media_t[0])/q);
    
  /* std::cout << "Matriz Ht\n";
  std:: cout << PGMATRIX_DATA(Matriz_Ht,1,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Ht,1,2) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Ht,1,3) << " \n";
  std:: cout << PGMATRIX_DATA(Matriz_Ht,2,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Ht,2,2) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Ht,2,3) << " \n";
  std:: cout << PGMATRIX_DATA(Matriz_Ht,3,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Ht,3,2) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Ht,3,3) << " \n"; */
  
  // Retornando o valor de sqrt(q)
  return raiz_q;
  
}

void Gera_Matriz_Kt(PGMATRIX Matriz_Kt, PGMATRIX Matriz_Ht, PGMATRIX Matriz_Qt, PGMATRIX Matriz_Covt)	
{
  // Variaveis
  PGMATRIX Matriz_St;
  PGMATRIX Matriz_St_Inv;
  PGMATRIX Matriz_Ht_Trans;
  PGMATRIX Matriz_Aux;
  PGMATRIX Matriz_Aux1;

  // Alocando memoria
  Matriz_St = PGMATRIX_ALLOC(3,3);
  Matriz_St_Inv = PGMATRIX_ALLOC(3,3);
  Matriz_Ht_Trans = PGMATRIX_ALLOC(3,3);
  Matriz_Aux = PGMATRIX_ALLOC(3,3);
  Matriz_Aux1 = PGMATRIX_ALLOC(3,3); 

  // Construindo a matriz St
  PGMATRIX_TRANSPOSE_COPY(Matriz_Ht_Trans, Matriz_Ht); 
  PGMATRIX_TRIPLEMULTIPLY_COPY(Matriz_Aux, Matriz_Ht, Matriz_Covt, Matriz_Ht_Trans, Matriz_Aux1); 	// Aux = Ht*Cov_t*Ht_Trans
  PGMATRIX_ADD_COPY(Matriz_St, Matriz_Aux, Matriz_Qt);						        // St = Aux + Qt

  // Construindo a matriz Kt
  PGMATRIX_INVERSE_COPY(Matriz_St_Inv, Matriz_St);
  PGMATRIX_TRIPLEMULTIPLY_COPY(Matriz_Kt, Matriz_Covt, Matriz_Ht_Trans, Matriz_St_Inv, Matriz_Aux1); 	// Kt = Cov_t*Ht_Trans*St_Inv

  PGMATRIX_FREE(Matriz_Aux);
  PGMATRIX_FREE(Matriz_Aux1);
  
  PGMATRIX_FREE(Matriz_St_Inv);
  PGMATRIX_FREE(Matriz_Ht_Trans);

  /*
  std::cout << "Matriz Kt\n";
  std:: cout << PGMATRIX_DATA(Matriz_Kt,1,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Kt,1,2) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Kt,1,3) << " \n";
  std:: cout << PGMATRIX_DATA(Matriz_Kt,2,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Kt,2,2) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Kt,2,3) << " \n";
  std:: cout << PGMATRIX_DATA(Matriz_Kt,3,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Kt,3,2) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_Kt,3,3) << " \n";	*/

  /*
  std::cout << "Matriz St\n";
  std:: cout << PGMATRIX_DATA(Matriz_St,1,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_St,1,2) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_St,1,3) << " \n";
  std:: cout << PGMATRIX_DATA(Matriz_St,2,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_St,2,2) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_St,2,3) << " \n";
  std:: cout << PGMATRIX_DATA(Matriz_St,3,1) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_St,3,2) << " ";
  std:: cout << PGMATRIX_DATA(Matriz_St,3,3) << " \n";	*/
  
  PGMATRIX_FREE(Matriz_St);
}

void Atualiza_Media_Covariancia_Sensores(double media_t[], double pos_landmark[], double z_landmark[], PGMATRIX Matriz_Kt, PGMATRIX Matriz_Covt, PGMATRIX Matriz_Ht, PGMATRIX Matriz_I, double rq)
{
  // Variaveis

  PGMATRIX Matriz_Zchapeu;
  PGMATRIX Matriz_Zt;
  PGMATRIX Matriz_Aux;
  PGMATRIX Matriz_Aux1;
  PGMATRIX Matriz_Aux2;
  PGMATRIX Matriz_Aux3;
  PGMATRIX Matriz_Aux4;

  // Alocando memoria
  Matriz_Zchapeu = PGMATRIX_ALLOC(3,1);
  Matriz_Zt = PGMATRIX_ALLOC(3,1);
  Matriz_Aux = PGMATRIX_ALLOC(3,1);
  Matriz_Aux1 = PGMATRIX_ALLOC(3,1);
  Matriz_Aux2 = PGMATRIX_ALLOC(3,3);
  Matriz_Aux3 = PGMATRIX_ALLOC(3,3);
  Matriz_Aux4 = PGMATRIX_ALLOC(3,3);

  // Construindo a Matriz Zchapeu

  PGMATRIX_DATA(Matriz_Zchapeu, 1, 1) = rq; 

  PGMATRIX_DATA(Matriz_Zchapeu, 2, 1) = atan2(pos_landmark[1] - media_t[1], pos_landmark[0] - media_t[0]) - media_t[2];
  
  PGMATRIX_DATA(Matriz_Zchapeu, 3, 1) = z_landmark[2];
  
  // Construindo a Matriz Zt
  
  PGMATRIX_DATA(Matriz_Zt, 1, 1) = z_landmark[0]; 
  PGMATRIX_DATA(Matriz_Zt, 2, 1) = z_landmark[1]; 
  PGMATRIX_DATA(Matriz_Zt, 3, 1) = z_landmark[2] + 10; 
    
  // Atualizando a Média com as Informações dos Sensores
  
  PGMATRIX_SUBSTRACT_COPY(Matriz_Aux, Matriz_Zt, Matriz_Zchapeu);
  PGMATRIX_MULTIPLY_COPY(Matriz_Aux1, Matriz_Kt, Matriz_Aux);

  media_t[0] = media_t[0] + PGMATRIX_DATA(Matriz_Aux1, 1, 1);
  media_t[1] = media_t[1] + PGMATRIX_DATA(Matriz_Aux1, 2, 1);
  media_t[2] = media_t[2] + PGMATRIX_DATA(Matriz_Aux1, 3, 1);
  
  // Atualizando a Convariancia com as Informações dos Sensores
  
  PGMATRIX_MULTIPLY_COPY(Matriz_Aux4, Matriz_Kt, Matriz_Ht);  
  PGMATRIX_SUBSTRACT_COPY(Matriz_Aux2, Matriz_I, Matriz_Aux4);  
  PGMATRIX_MULTIPLY_COPY(Matriz_Aux3, Matriz_Aux2, Matriz_Covt);
  PGMATRIX_COPY(Matriz_Covt, Matriz_Aux3); 

  // Liberando a memoria alocada
  
  PGMATRIX_FREE(Matriz_Zchapeu); 
  PGMATRIX_FREE(Matriz_Zt);
  PGMATRIX_FREE(Matriz_Aux);
  PGMATRIX_FREE(Matriz_Aux1);
  PGMATRIX_FREE(Matriz_Aux2);
  PGMATRIX_FREE(Matriz_Aux3);
  PGMATRIX_FREE(Matriz_Aux4);

}














